#include "WorkpieceE575.h"

#include <QDebug>
#include <QThread>
#include <QTime>


#include <unistd.h>
#include <iostream>
using std::cout;

#include "../mesurementinterface.h"
#include "../FitAdjust.hpp"

#define     MAX_THREAD_NUM_E575        8

//#ifndef MEASURE_TIME
//#define MEASURE_TIME
//extern QMap<QString, QTime*>                   g_measurementCostTime;
//#endif

WorkpieceE575::WorkpieceE575() : m_threadStopFlag(false),
                                 m_initThreadFinished_(false),
                                 m_isSetPathFlag(false)
{
    m_workpieceBuff.SetSize(MAX_WORKPIECE_BUFFER_NUM);

    if(!m_initThreadFinished_) {
        InitThread();
    }
}

WorkpieceE575::~WorkpieceE575()
{
    qDebug() << "start to realse E575 ";
    ExitThread();
    qDebug() << "finish to realse E575";
}

bool WorkpieceE575::SetTemplatePath(const std::string &path)
{
    qDebug() << "E575: template path : " << QString::fromStdString(path);
    AlgBase::SetTemplatePath(path);
    m_isSetPathFlag = true;
    qDebug() << "set template path success";
    return true;
}

bool WorkpieceE575::SetStandardVal(const std::map<string, MeasureItemStandardVal> &standardValMap)
{
    std::map<std::string, float> standardValStrMap;
    for(const auto &item: standardValMap) {
        std::string keyStr = item.first;
        standardValStrMap.insert(make_pair(keyStr, item.second.standardVal));
    }
    AlgBase::SetStandardVal(standardValStrMap);
    return true;
}

void WorkpieceE575::SetWorkpieceInfo(const string &modeName, const string &workpieceType)
{
    AlgBase::SetWorkpieceInfo(modeName, workpieceType);
}

void WorkpieceE575::CacheData(const Mat &image, const QPoint &datumOffset, int workpieceId, int pictureId, int channelId, int fixtureId)
{
    qDebug() << "E575: cache Data";
    if(!m_workpieceMap.contains(workpieceId))
    {
        WorkpieceInfo workpieceInfo;
        workpieceInfo.workpieceId  = workpieceId            ;
        workpieceInfo.channelId    = channelId              ;
        workpieceInfo.fixtureId    = fixtureId              ;
        m_workpieceMap.insert(workpieceId, workpieceInfo)   ;
    }
    switch (pictureId) {
    case PICTURE_ID_1:
    case PICTURE_ID_2:
    case PICTURE_ID_5:
        m_workpieceMap[workpieceId].imageMap[pictureId]            = image;
        m_workpieceMap[workpieceId].imageDatumOffsetMap[pictureId] = datumOffset;
        m_workpieceMap[workpieceId].imageNum++;
        m_workpieceMap[workpieceId].curPictureId = pictureId  ;
        break;
    default:
        break;
    }
    // if finished input to queue
    //判断当前图像是否可以计算，如果可以计算，发送信号进行计算
    if(m_workpieceMap[workpieceId].IsPictureUseable(WORKPIECE_TYPE_E575, pictureId)) {
        m_workpieceBuff.SetData(m_workpieceMap[workpieceId]);

        if(m_workpieceMap[workpieceId].IsWorkpieceUseable(WORKPIECE_TYPE_E575)) {
            m_workpieceMap.remove(workpieceId);
        }
        //
        // 此处默认工件号由小到大进行排序，如果当前工件号和最小工件号相差MAX_WAIT_WORKPIECE_NUM个，
        // 说明最小工件号的工件图像已经丢失，
        // 需要删除该工件信息
        QMap<int, WorkpieceInfo>::iterator iter = m_workpieceMap.begin();
        int minWorkpieceId = iter.key();
        if(workpieceId - minWorkpieceId > MAX_WAIT_WORKPIECE_NUM) {
            m_workpieceMap.remove(minWorkpieceId);
            qDebug() << "workpiece : " << workpieceId << "lost";
        }
    }
}

void WorkpieceE575::ProcessData()
{
    while(!m_isSetPathFlag) {
        sleep(0.1);
    }
    qDebug() << "E575 measure workpiece";

    AlgBackDatum op0(MAX_THREAD_NUM_E575);
    AlgBackDownCircle op1(MAX_THREAD_NUM_E575);
    AlgBackLeftCircle op2(MAX_THREAD_NUM_E575);
    AlgBackRightCircle op3(MAX_THREAD_NUM_E575);
    AlgBackS4S5S6LDmeasure op4(MAX_THREAD_NUM_E575);
    AlgBackS1S2RectEdge op5(MAX_THREAD_NUM_E575);
    AlgBackSS1SS2SmallCircle  op6(MAX_THREAD_NUM_E575);
    AlgFrontSE3SE4SE5SE6SE7SE8EdgeOuter op7(MAX_THREAD_NUM_E575);
    AlgFrontSS3toSS8SmallCircle op8(MAX_THREAD_NUM_E575);
    AlgBackS3S4LDmeasure op9(MAX_THREAD_NUM_E575);
    AlgHighSE1SE9EdgeOuter op10(MAX_THREAD_NUM_E575);

    int id = -1;

    while(!m_threadStopFlag)
    {
        WorkpieceInfo workpieceInfo;
        if(!m_workpieceBuff.GetData(workpieceInfo)) {
            qDebug() << "current thread exit" << QThread::currentThreadId();
            return;
        }

        // process data
        cv::Mat img  = workpieceInfo.imageMap[PICTURE_ID_1].clone();
        cv::Mat imgFront = workpieceInfo.imageMap[PICTURE_ID_2].clone();
        cv::Mat imgHigh  = workpieceInfo.imageMap[PICTURE_ID_5].clone();


        int workpieceId = workpieceInfo.workpieceId;
        int channelId   = workpieceInfo.channelId  ;
        int fixtureId   = workpieceInfo.fixtureId  ;

#ifdef MEASURE_TIME
        auto RecordStartTime = [&](int workpieceId, int pictureId) {
            QString key = QString("%1-%2").arg(workpieceId).arg(pictureId);
            QTime *startTime = new QTime();
            g_measurementCostTime[key] = startTime;
            startTime->start();
        };

        RecordStartTime(workpieceId, PICTURE_ID_1);
        RecordStartTime(workpieceId, PICTURE_ID_2);
        RecordStartTime(workpieceId, PICTURE_ID_5);
#endif

        id++;
        id = id%MAX_THREAD_NUM_E575;

        std::vector<std::map<string, float>> measResBack(7);
        std::vector<std::map<string, float>> measResFront(2);
        std::map<string, float> measResHigh;

        std::thread t0(op0,  std::ref(img),      std::ref(id));
        std::thread t1(op1,  std::ref(img),      std::ref(id), std::ref(measResBack[0]), std::ref(fixtureId));
        std::thread t2(op2,  std::ref(img),      std::ref(id), std::ref(measResBack[1]), std::ref(fixtureId));
        std::thread t3(op3,  std::ref(img),      std::ref(id), std::ref(measResBack[2]), std::ref(fixtureId));
        std::thread t4(op4,  std::ref(img),      std::ref(id), std::ref(measResBack[3]), std::ref(fixtureId));
        std::thread t5(op5,  std::ref(img),      std::ref(id), std::ref(measResBack[4]), std::ref(fixtureId));
        std::thread t6(op6,  std::ref(img),      std::ref(id), std::ref(measResBack[5]), std::ref(fixtureId));
        std::thread t7(op7,  std::ref(imgFront), std::ref(id), std::ref(measResFront[0]), std::ref(fixtureId));
        std::thread t8(op8,  std::ref(imgFront), std::ref(id), std::ref(measResFront[1]), std::ref(fixtureId));
        std::thread t9(op9,  std::ref(img),      std::ref(id), std::ref(measResBack[6]), std::ref(fixtureId));
        std::thread t10(op10,std::ref(imgHigh),  std::ref(id), std::ref(measResHigh)   , std::ref(fixtureId));

        t0.join();
        t1.join();
        t2.join();
        t3.join();
        t4.join();
        t5.join();
        t6.join();
        t7.join();
        t8.join();
        t9.join();
        t10.join();

        auto MergeResult = [&](std::map<string, float> &measureInfo,
                std::map<string, float> measureInfoSub) {
            for(const auto &item : measureInfoSub) {
                measureInfo.insert(make_pair(item.first, item.second));
            }
        };

        QMap<QString, QString> error;
        std::map<std::string, float> measResBackS;

        for(auto &item: measResBack) {
            MergeResult(measResBackS, item);
        }

        CorrectThreeD(measResBackS, fixtureId);

        emit MesurementResultResponse(workpieceId,
                                      PICTURE_ID_1,
                                      channelId,
                                      fixtureId,
                                      measResBackS,
                                      error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_1
                 << "channelId: "
                 << channelId;

        std::map<std::string, float> measResFrontS;
        for(auto& item: measResFront) {
            MergeResult(measResFrontS, item);
        }

        CorrectThreeD(measResFrontS, fixtureId);
        emit MesurementResultResponse(workpieceId,
                                      PICTURE_ID_2,
                                      channelId,
                                      fixtureId,
                                      measResFrontS,
                                      error);


        std::map<std::string, float>   measureInfoHigh;
        MergeResult(measureInfoHigh, measResHigh);

        CorrectThreeD(measureInfoHigh, fixtureId);
        emit MesurementResultResponse(workpieceId, PICTURE_ID_5, channelId,
                                      fixtureId, measureInfoHigh, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_5
                 << "channelId: "
                 << channelId;

        AlgBase::clearAllflag(id);
        qDebug() << "process data send signal";
    }
}

bool WorkpieceE575::GetDatumPixelDistance(const QImage &img, int fixtureId, QMap<QString, float> &pixelDistance)
{
    return false;
}

void WorkpieceE575::InitThread()
{
    for(int i = 0; i < 1; i++) {
        qDebug() << "start thread";
        m_threadPool.emplace_back(std::thread(&WorkpieceE575::ProcessData, this));
    }
    qDebug() << "start thread success";
    m_initThreadFinished_ = true;
}

void WorkpieceE575::ExitThread()
{
    if(m_initThreadFinished_) {
        if(!m_threadStopFlag) {
            m_threadStopFlag = true;
        }
        qDebug() << "start to exit thread";

        int loopCount = 0;
        while(!m_workpieceBuff.Empty() && loopCount < 3) {
            sleep(1);
            loopCount++;
        }

        // 调用该语句释放条件变量的等待条件
        m_workpieceBuff.Exit();

        for(size_t i = 0; i < m_threadPool.size(); ++i) {
            qDebug() << "workpiece E575 line 252 ";
            qDebug() << "m thread init finish: " << m_initThreadFinished_;
            qDebug() << "m thread stop flag: " << m_threadStopFlag ;
            m_threadPool[i].join();
        }
        qDebug() << "measurement thread exit safely!";
    }
}

void WorkpieceE575::CorrectThreeD(std::map<string, float> &measRes, int fixtureId)
{
    if(MeasurementInterface::m_isDataCorrect) {
        QMap<QString, MeasurementInterface::MeasureInfo> qMapInfo;
        for (const auto &item : measRes)
        {
            QString &&name = QString::fromStdString(item.first);
            float dist = item.second;

            auto strlist = name.split('_');
            QString itemName = strlist[0];
            if (strlist.back() == "pixel")
                qMapInfo[itemName].pixel_distance = measRes[item.first];
            else if(strlist.back() == "real"){
                qMapInfo[itemName].real_distance = measRes[item.first];
            } else {
                // position not process
            }
        }

        QMap<QString, MeasurementInterface::MeasureInfo>::ConstIterator iter = qMapInfo.begin();
        vector<AdjustDataInfo> d;
        for(iter; iter != qMapInfo.end(); iter++) {
            std::string keyTempStr = iter.key().toStdString();
            multimap<double, double> datas;
            datas.insert(make_pair(iter.value().pixel_distance, iter.value().real_distance));
            AdjustDataInfo adjustDataInfo = {keyTempStr, datas};
            d.emplace_back(adjustDataInfo);
        }

        vector<AdjustDataInfo>* ptr = new vector<AdjustDataInfo>(d);

        std::string path;
        if(0 == fixtureId) {
            path = (AlgBase::m_dir_distance_fitting_model + "/test7.xml");
        }


        PixelOutliersClassify adjust(path);
        adjust.adjustOutlierData(ptr);
        auto change_data = *ptr;
        for(auto& item: change_data) {
            for(auto& data: item.item_data) {
                std::string faiItemName = item.item_name;
                std::string faiItemNameReal = faiItemName + "_real";
                measRes[faiItemNameReal] = data.second;
            }
        }
    }
}

float WorkpieceE575::CalPosition(std::map<string, float> &measRes, string strX, string strY)
{
    if(measRes.empty()) {
        qDebug() << "gray algorithm: E575 caculate position error";
        return -1;
    }
    int endIndexX = strX.find("_real");
    int endIndexY = strY.find("_real");
    std::string standardXStr = strX.substr(0, endIndexX);
    std::string standardYStr = strY.substr(0, endIndexY);
    return 2*sqrt(((measRes[strX] - AlgBase::m_standardValMap[standardXStr]) * (measRes[strX] - AlgBase::m_standardValMap[standardXStr]))
                  + ((measRes[strY] - AlgBase::m_standardValMap[standardYStr]) * (measRes[strY] - AlgBase::m_standardValMap[standardYStr])));
}
